//
// Created by bubble on 2022/1/8.
//

#ifndef HERORM2022_KALMAN_HPP
#define HERORM2022_KALMAN_HPP

#include <Eigen/Dense>
#include "rclcpp/rclcpp.hpp"
//#include "../../../tool/include/util_func.h"

#define PI_KF 3.1415926535897
template<int V_X, int V_Z>
class Kalman {
 public:
    using Matrix_zzd = Eigen::Matrix<double, V_Z, V_Z>;
    using Matrix_xxd = Eigen::Matrix<double, V_X, V_X>;
    using Matrix_zxd = Eigen::Matrix<double, V_Z, V_X>;
    using Matrix_xzd = Eigen::Matrix<double, V_X, V_Z>;
    using Matrix_x1d = Eigen::Matrix<double, V_X, 1>;
    using Matrix_z1d = Eigen::Matrix<double, V_Z, 1>;
    int64_t last_t{0};// 单位ms
    double test1 = 0;double test2 = 0; double test3 = 0;
    //double_t t_everage = 0;

 public:
    Matrix_x1d X;// k-1时刻的滤波值，即是k-1时刻的值
    Matrix_xzd K;// Kalman增益
    Matrix_xxd A;// 转移矩阵
    Matrix_zxd H;// 观测矩阵
    Matrix_xzd G;
    Matrix_zzd U;
    Matrix_xxd Q;// 预测过程噪声偏差的方差
    Matrix_zzd R;// 测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
    Matrix_xxd P;// 估计误差协方差
    double Distance_change{0};
 public:
    Kalman()
    {
        A.setIdentity();
        R = R.setIdentity();
        R(0, 0) = 4 * pow(180 / PI_KF, 2);// 角度测量值方差
        R(1, 1) = 400 * pow(180 / PI_KF, 2);// 角度测量值方差
        R(2, 2) = 4 * pow(180 / PI_KF, 2);// 角度测量值方差

        Q = Q.setIdentity();
        Q(0, 0) = 33;
        Q(1, 1) = 328280;
        Q(2, 2) = 33000;
        Q(3, 3) = 1 ;
        Q(5, 5) = 0.01 * pow((180.0 / PI_KF), 2);
        Q(6, 6) = 100.0 * pow((180.0 / PI_KF), 2);
        
        U << 100000000,0,0,
             0,17000000,0,
             0,0,24000000;

        P = P.setIdentity();
        H << 1, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 1, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 1, 0;
    }
    bool judgeDistanceDelta(double distance){
        //算错
        if(distance - X(3, 0) > 100 && Distance_change < 5)
        {
            Distance_change ++;
            return false;
        }
        //Distance_change大于4时，强制重置Distance
        else if(Distance_change > 4)
        {
            Distance_change = 0;
            X(3, 0) = distance;
        }
        return true;
    }

    Kalman(Matrix_xxd A, Matrix_zxd H, Matrix_xxd Q, Matrix_zzd R, Matrix_x1d init, double t) { reset(A, H, Q, R, init, t); }

    //没用到
    void reset(Matrix_xxd A, Matrix_zxd H, Matrix_xxd Q, Matrix_zzd R, Matrix_x1d init, double t)
    {
        this->A = A;
        this->H = H;
        this->P = Matrix_xxd::Zero();
        this->Q = Q;
        this->R = R;
        X = init;
        last_t = t;
    }

    void reset(Matrix_x1d init, int64_t t)
    {
        X = init;
        last_t = t;
    }

    void reset(double yaw ,double distance ,double pitch)
    {
        A.setIdentity();
        P.setIdentity();
        K.setIdentity();
        X << yaw, 0.0, 0.0, distance, 0.0,0.0, pitch, 0.0;
        //last_t = t;
    }

    Matrix_x1d update(Matrix_z1d z_k, int64_t t)
    {

        if (t - last_t < 0)
        {
//            LOGE("fatal error, if you saw this error happened, please report zwx(QQ:129551827).");
            exit(-1);
        }
        double dt = (t-last_t)*0.001;
        // 设置转移矩阵中的时间项
        A(0, 1) = static_cast<double>(dt);
        A(0, 2) = static_cast<double>(0.5 * dt * dt);
        // A(0, 2) = static_cast<double>((t - last_t)* 0.001);
        A(1, 2) = static_cast<double>(dt);
        A(3, 4) = static_cast<double>(dt);
        A(4, 5) = static_cast<double>(dt);
        A(6, 7) = static_cast<double>(dt);

        // dt = 0.007;

        // G <<pow(dt,3)/6,0,0,
        //     pow(dt,2)/2,0,0,
        //     dt,0,0,
        //     0,pow(dt,3)/6,0,
        //     0,pow(dt,2)/2,0,
        //     0,dt,0,
        //     0,0,pow(dt,2)/2,
        //     0,0,dt;

        // Q = G*U*G.transpose();

        // test1 += Q(0,0);
        // test2 += Q(3,3);
        // test3 += Q(6,6);

        // test1 = 0;
        // test2 = Q(3,3);
        // test3 = Q(6,6);

        // std::ofstream fout;
        // std::string save_addr = "test.csv";
        // fout.open(save_addr, std::ios::app | std::ios::out);
        // fout << test1 << " " << test2 << " " << test3 <<std::endl;
        // fout.close();

        last_t = t;

        // 预测下一时刻的值
        Matrix_x1d p_x_k = A * X;// x 的先验估计由上一个时间点的后验估计值和输入信息给出

        // 求协方差
        P = A * P * A.transpose() + Q;// 计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q

        // 计算kalman增益
        K = P * H.transpose() * (H * P * H.transpose() + R).inverse();// Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
        // 修正结果，即计算滤波值
        X = p_x_k + K * (z_k - H * p_x_k);// 利用残余的信息改善对 x(t) 的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
        // 更新后验估计
        P = (Matrix_xxd::Identity() - K * H) * P;// 计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]
        return X;
        }
};

#endif//HERORM2022_KALMAN_HPP
